//
//  TouchManager.hpp
//  cali
//
//  Created by Fan Qu on 10/14/19.
//  Copyright © 2019 GF. All rights reserved.
//

#ifndef TouchManager_hpp
#define TouchManager_hpp

#include <vector>
#include <cstdlib>
#include <ctime>
#include <QObject>
#include "utils.h"
#include "TouchEvent.hpp"


//#define LOG_TOUCH

#define OnePointGate 30.0
#define MiniOnePointDate 30.0

#define MiniTimeDurGate std::chrono::milliseconds(60)

#define dur_cast_milli(x) std::chrono::duration_cast<std::chrono::milliseconds>((x))

typedef std::chrono::time_point<std::chrono::high_resolution_clock> HighResoClock;

class EventCallBack: public QObject{
Q_OBJECT
public:
    virtual void onSuccess(TouchEvent *event) = 0;
    
    virtual ~EventCallBack() {}
};

class TouchManager {

protected:
    const int MAX_POINT_ID = 100000;
    std::vector<TouchPoint> points;
    std::vector<TouchPoint> pointsToAdd;
    //std::vector<int> pointsID;
    EventCallBack *touchUpCallBack, *touchDownCallBack, *touchMoveCallBack;
    
    // get New PointsID for new point
    int getNewPointsID() {
        int returnId = -1;
        bool dupliFlag = false;
        do{
            int tempId = rand() % MAX_POINT_ID;
            
            for(int i = 0; i < points.size(); ++i) {
                if(points[i].id == tempId) {
                    dupliFlag = true;
                    break;
                }
            }
            if(!dupliFlag) {
                returnId = tempId;
            }
        }while(dupliFlag);
        return returnId;
    }
    
public:
    void setTouchUpCallBack(EventCallBack *callback) {
        this->touchUpCallBack = callback;
    }
    
    void setTouchDownCallBack(EventCallBack *callback) {
        this->touchDownCallBack = callback;
    }
    
    void setTouchMoveCallBack(EventCallBack *callback) {
        this->touchMoveCallBack = callback;
    }
    
    TouchManager(): touchUpCallBack(NULL), touchDownCallBack(NULL), touchMoveCallBack(NULL) {
        srand((unsigned)time(0));
    }
    
    void handleFrame(const std::vector<Pos2F>& newPoints) {
            //std::vector<Pos2F> tempPts = points;
            pointsToAdd.clear();
            std::vector<int> tempPNum;
            std::vector<int> oldPIndex, canHaveParent;
            std::vector<int> hasMatched;
            for(int i = 0; i < newPoints.size(); ++i) {
                oldPIndex.push_back(-1);
                canHaveParent.push_back(-1);
                hasMatched.push_back(-1);
            }
            for(int i = 0; i < points.size(); ++i) {
                tempPNum.push_back(0);
            }
            bool stopFlag = false;
            auto now_clock = std::chrono::high_resolution_clock::now();

            bool shouldEndl = false;
            while(!stopFlag) {

                for(int i = 0; i < points.size(); ++i) {
                    if(tempPNum[i] != -1)
                        tempPNum[i] = 0;
                }
                for(int i = 0; i < newPoints.size(); ++i)
                    oldPIndex[i] = -1;
                for(int i = 0; i < newPoints.size(); ++i) {
                    if(hasMatched[i] == -1) {
                        double minDis = 999999.0;
                        int pointIndex = -1, bestTimeIndex = -1, ableToHavePast = -1;
                        for (int k = 0; ; ++k) {
                            int pointCount = 0;
                            for(int j = 0; j < points.size(); ++j) {
                                if (points[j].historyClocks.size() > k) {
                                    size_t timeIndex = points[j].historyClocks.size() - 1 - k;
                                    pointCount++;
                                    shouldEndl = true;
                                    double tempDis = getDis2f(newPoints[i], points[j].historyPoses[timeIndex]);
    #ifdef LOG_TOUCH
                                    std::cout << " i = " << i << " j = " << j << " k = " << k << " time dur: " << dur_cast_milli(now_clock - points[j].historyClocks[timeIndex]).count() << " dis: " << tempDis << "points size" << points.size() << std::endl;
    #endif
                                    if (dur_cast_milli(now_clock - points[j].historyClocks[timeIndex]) > MiniTimeDurGate) {
                                        continue;
                                    }


                                    if(tempPNum[j] != -1 && tempDis < minDis && tempDis < OnePointGate && (k == 0 || pointIndex == -1)) {
                                        minDis = tempDis;
                                        pointIndex = j;
                                        bestTimeIndex = static_cast<int>(timeIndex);
    #ifdef LOG_TOUCH
                                        std::cout << " i = " << i << " j = " << j << " k = " << k << " minDis = " << minDis << std::endl;
    #endif
                                    }
                                    if (tempDis < MiniOnePointDate) {
    #ifdef LOG_TOUCH
                                        std::cout << " i = " << i << " j = " << j << " k = " << k << " able to have past" << std::endl;
                                        ableToHavePast = 1;
    #endif
                                    }
                                }

                            }
                            if (pointCount == 0) {
                                break;
                            }
                        }
                        if(pointIndex != -1) {
                            oldPIndex[i] = pointIndex;
                            tempPNum[pointIndex]++;
                        }
                        if (ableToHavePast != -1) {
                            canHaveParent[i] = 1;
                        }
                    }

                }

                bool canClearFlag = true;


                for(int i = 0; i < points.size(); ++i) {
                    if(tempPNum[i] > 1) {

                        canClearFlag = false;
                        double minDis = 999999.0;
                        int minNewIndex = -1;
                        for(int j = 0; j < newPoints.size(); ++j) {
                            if(hasMatched[j] == -1 && oldPIndex[j] == i) {
                                double tempDis = getDis2f(newPoints[j], points[i].pos);

                                if(tempDis < minDis) {
                                    minDis = tempDis;
                                    minNewIndex = j;
                                }
                            }
                        }
                        hasMatched[minNewIndex] = i;
    #ifdef LOG_TOUCH
                        std::cout << " i = " << i << " tempPNum = " << tempPNum[i] << " minNewIndex:" << minNewIndex << std::endl;
    #endif
                        tempPNum[i] = -1;
                    }
                }
                if(canClearFlag) {
                    for(int i = 0; i < newPoints.size(); ++i) {
                        if(hasMatched[i] == -1 && oldPIndex[i] != -1) {
                            hasMatched[i] = oldPIndex[i];
                            tempPNum[oldPIndex[i]] = -1;
                        }
                    }

                    std::vector<int> toDeletePts;
                    int tempI = 0;
                    for(auto it = points.begin(); it != points.end(); tempI++) {
                        //&& dur_cast_milli(now_clock - it->historyClocks.back()) > MiniTimeDurGate
                        if(tempPNum[tempI] != -1 && dur_cast_milli(now_clock - it->historyClocks.back()) > MiniTimeDurGate ) {
                            eraseOldPoint(*it);
                            toDeletePts.push_back(tempI);
                            it++;
                            //it = points.erase(it);
                        }
                        else
                            it++;
                    }

                    for(int i = 0; i < newPoints.size(); ++i) {
                        if(hasMatched[i] == -1 && canHaveParent[i] == -1) {
                            addNewPoint(newPoints[i], now_clock);
                        }
                    }

                    for(int i = 0; i < newPoints.size(); ++i) {
                        if(hasMatched[i] != -1) {
                            Pos2F lastPos = points[hasMatched[i]].pos;
                            points[hasMatched[i]].trace.push_back(lastPos);
                            points[hasMatched[i]].pos = newPoints[i];
                            points[hasMatched[i]].historyClocks.push_back(now_clock);
                            points[hasMatched[i]].historyPoses.push_back(newPoints[i]);
                            handleTouchMove(points[hasMatched[i]]);
                        }
                    }

                    //delete old Points
                    tempI = 0;
                    int toDeleteIndex = 0;
                    for(auto it = points.begin(); it != points.end() && toDeleteIndex < toDeletePts.size(); ++tempI) {
                        if(tempI == toDeletePts[toDeleteIndex]) {
                            //it++;
                            toDeleteIndex++;
                            it = points.erase(it);
                        }
                        else
                            it++;
                    }

                    // Add new Points
                    for(auto it = pointsToAdd.begin(); it != pointsToAdd.end(); ++it) {
                        points.push_back(*it);
                    }
                    stopFlag = true;
                }
            }
    #ifdef LOG_TOUCH
            if (shouldEndl)
                std::cout << std::endl;
    #endif
        }

        void addNewPoint(const Pos2F& pos, const HighResoClock &now) {
            TouchPoint newPoint(pos, getNewPointsID());
            newPoint.historyClocks.push_back(now);
            newPoint.historyPoses.push_back(pos);
            pointsToAdd.push_back(newPoint);
            if(touchDownCallBack != NULL) {
                TouchDown* event = new TouchDown(newPoint.id, pos.x, pos.y, newPoint);
                touchDownCallBack->onSuccess(event);
            }
        }

        void eraseOldPoint(const TouchPoint& point) {
            TouchPoint tempPoint = point;
            tempPoint.trace.push_back(point.pos);
            if(touchUpCallBack != NULL) {
                TouchUp* event = new TouchUp(tempPoint.id, tempPoint);
                touchUpCallBack->onSuccess(event);
            }


        }

        void handleTouchMove(const TouchPoint& point) {
            TouchPoint tempPoint = point;
            tempPoint.trace.push_back(point.pos);
            if(touchMoveCallBack != NULL) {
                TouchMove* event = new TouchMove(tempPoint.id, tempPoint);
                touchMoveCallBack->onSuccess(event);
            }
        }

        ~TouchManager() {
            if(touchUpCallBack != NULL)
                delete touchUpCallBack;
            if(touchDownCallBack != NULL)
                delete touchUpCallBack;
            if(touchMoveCallBack != NULL)
                delete touchUpCallBack;
        }
    
    
};

#endif /* TouchManager_hpp */
